
#ifndef PLANNER_PKG__NAVFN_HPP_
#define PLANNER_PKG__NAVFN_HPP_

#include <math.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>

namespace planner_pkg
{

// cost defs
   // navfn cost values are set to
    // COST_NEUTRAL + COST_FACTOR * costmap_cost_value.
    // Incoming costmap cost values are in the range 0 to 252.
    // With COST_NEUTRAL of 50, the COST_FACTOR needs to be about 0.8 to
    // ensure the input values are spread evenly over the output range, 50
    // to 253.  If COST_FACTOR is higher, cost values will have a plateau
    // around obstacles and the planner will then treat (for example) the
    // whole width of a narrow hallway as equally undesirable and thus
    // will not plan paths down the center.
#define COST_UNKNOWN_ROS 255 // 255 is unknown cost 未知成本
#define COST_OBS 254         // 254 for forbidden regions 禁止区域
#define COST_OBS_ROS 253     // ROS values of 253 are obstacles 障碍物

#define COST_NEUTRAL 50 // Set this to "open space" value 开放空间值,中立成本值
#define COST_FACTOR 0.8 // Used for translating costs in NavFn::setCostmap() 成本转换因子

// Define the cost type in the case that it is not set. However, this allows
// clients to modify it without changing the file. Arguably, it is better to require it to
// be defined by a user explicitly
#ifndef COSTTYPE
#define COSTTYPE unsigned char // Whatever is used...
#endif

// potential defs
#define POT_HIGH 1.0e10 // 未分配单元格的势能

// priority buffers
#define PRIORITYBUFSIZE 10000 //缓冲区大小

    /**
     * @class NavFn
     * @brief Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based.
     *  Origin is upper left, x is right, y is down.
     */
    class NavFn
    {
    public:
        /**
         * @brief  Constructs the planner
         * @param nx The x size of the map
         * @param ny The y size of the map
         */
        NavFn(int nx, int ny);
        //析构函数
        ~NavFn();

        /**
         * @brief  设置地图尺寸
         * @param nx The x size of the map
         * @param ny The y size of the map
         */
        void setNavArr(int nx, int ny);
        int nx, ny, ns; //size of grid, in pixels 地图尺寸

        /**
         * @brief 设置成本地图 Set up the cost array for the planner, usually from ROS
         * @param cmap The costmap
         * @param allow_unknown Whether or not the planner should be allowed to plan through
         *   unknown space
         */
        void setCostmap(const COSTTYPE *cmap, bool allow_unknown = true);

        /**
         * @brief 计算导航函数 Calculates a plan using the A* heuristic, returns true if one is found
         * @return True if a plan is found, false otherwise
         */
        bool calcNavFnAstar();

        float *getPathX();
        float *getPathY();

        /**
         * @brief  获取路径长度
         * @return The length of a path, 0 if not found
         */
        int getPathLen();

        /**
         * @brief  获取上次计算路径的成本
         * @return The cost of the last path found
         */
        float getLastPathCost();

        /** cell arrays */
        COSTTYPE *costarr; //成本数组
        float *potarr;     //势能数组
        bool *pending;     //待处理数组
        int nobs;          //障碍物单元格数量

        /** block priority buffers */
        int *pb1, *pb2, *pb3;      //分配优先级缓冲区
        int *curP, *nextP, *overP; //当前、下一个、溢出 优先级缓冲区
        int curPe, nextPe, overPe; //优先级缓冲区终点

        /** block priority thresholds */
        float curT;   //当前阈值
        float priInc; //优先级增量

        /** goal and start positions */
        /**
         * @brief  Sets the goal position for the planner.
         * Note: the navigation cost field computed gives the cost to get to a given point
         * from the goal, not from the start.
         * @param goal the goal position
         */
        void setGoal(int *goal);

        /**
         * @brief  Sets the start position for the planner.
         * Note: the navigation cost field computed gives the cost to get to a given point
         * from the goal, not from the start.
         * @param start the start position
         */
        void setStart(int *start);

        int goal[2];
        int start[2];
        /**
         * @brief  初始化单元格成本
         * @param k the cell to initialize
         * @param v the cost to give to the cell
         */
        void initCost(int k, float v);

        /** propagation */

        /**
         * @brief  Updates the cell at index n using the A* heuristic
         * @param n The index to update
         */
        void updateCellAstar(int n);

        /**
         * @brief  Set up navigation potential arrays for new propagation
         * @param keepit whether or not use COST_NEUTRAL
         */
        void setupNavFn(bool keepit = false);

        /**
         * @brief  Run propagation for <cycles> iterations, or until start is reached using
         * the best-first A* method with Euclidean distance heuristic
         * @param cycles The maximum number of iterations to run for
         * @return true if the start point is reached
         */
        bool propNavFnAstar(int cycles); /**< returns true if start point found */

        /** gradient and paths */
        float *gradx, *grady; //梯度数组
        float *pathx, *pathy; //路径点数组
        int npath;            //路径点数量
        int npathbuf;         //路径点缓冲区大小

        float last_path_cost_; //上次计算的路径成本

        /**
         * @brief 计算路径 Calculates the path for at mose <n> cycles
         * @param n The maximum number of cycles to run for
         * @return The lenght of the path found, 0 if none
         */
        int calcPath(int n, int *st = NULL);

        /**
         * @brief 计算单元格梯度 Calculate gradient at a cell
         * @param n Cell number <n>
         * @return float norm
         */
        float gradCell(int n); /**< calculates gradient at cell <n>, returns norm */

        float pathStep; //梯度跟随步长

    private:
        
    };

} // namespace planner_pkg

#endif // PLANNER_PKG__NAVFN_HPP_
